/**
 * \file Matrix.h
 * \brief Matrix definition and declaration class
 * \author Edouard ROGE
 * \date 13 decembre 2010
 */

#ifndef _MATRIX_
#define _MATRIX_

#include <string.h>
#include "Quaternion.h"
#include "Point3D.h"
#include "Vector3D.h"

namespace Math
{
	namespace M3D
	{
		/**
		 * \class Matrix
		 * \brief class with template to instantiate a Matrix  (4 x 4)
		 */
		template < class T >
		class Matrix : public Core::Memory::HeapObject
		{
		public:
			/**
			 * \fn Matrix();
			 * \brief Default constructor
			 */
			Matrix();

			/**
			 * \fn Matrix(const Matrix<T> & _mMatrix);
			 * \param _mMatrix Matrix to copy
			 * \brief Copy constructor
			 */
			Matrix(const Matrix<T> & _mMatrix);

			/**
			 * \fn Matrix(T _tab[16]);
			 * \param _tab fill Matrix with value of _tab
			 * \brief constructor
			 */
			Matrix(T _tab[16]);

			/**
			 * \fn Matrix(T _tab[4][4]);
			 * \param _tab fill Matrix with value of _tab
			 * \brief constructor
			 */
			Matrix(T _tab[4][4]);

			/**
			 * \fn ~Matrix();
			 * \brief Destructor
			 */
			~Matrix();

			/**
			 * \fn void CreateTranslation(T _x, T _y, T _z);
			 * \param _x x value translated
			 * \param _y y value translated
			 * \param _z z value translated
			 * \brief create translation matrix according to values in parameter
			 */
			void CreateTranslation(T _x, T _y, T _z);

			/**
			 * \fn void CreateTranslation( const Vector3D<T> & _translation );
			 * \param _translation translation vector
			 * \brief create translation matrix according to value in parameter
			 */
			void CreateTranslation(const Vector3D<T> & _translation);

			/**
			 * \fn void CreateRotation( const Quaternion & _q );
			 * \param _q quaternion
			 * \brief create rotation matrix according to the quaternion passed in parameter
			 */
			void CreateRotation(const Quaternion & _q);

			/**
			 * \fn void CreateRotation( const Vector3D<float> & _fAxis, float _fAngle );
			 * \param _fAxis rotation axis
			 * \param _fAngle rotation angle
			 * \brief create rotation matrix according to the values passed in parameter
			 */
			void CreateRotation(const Vector3D<float> & _fAxis, float _fAngle);

			/**
			 * \fn void CreateScale( const Vector3D<T> & _scale );
			 * \param _scale 3D scale factor
			 * \brief create scale matrix according to vector3D passed in parameter
			 */
			void CreateScale(const Vector3D<T> & _scale);

			/**
			 * \fn void CreateScale( T _scaleX, T _scaleY, T _scaleZ );
			 * \param _scaleX x scale factor
			 * \param _scaleY y scale factor
			 * \param _scaleZ z scale factor
			 * \brief create scale matrix according to values passed in parameter
			 */
			void CreateScale(T _scaleX, T _scaleY, T _scaleZ);

			/**
			 * \fn void CreateProjection( T _fFov, T _fAspect, T _fZNear, T _fZFar );
			 * \param _fFov field of view
			 * \param _fAspect aspect ratio
			 * \param _fZNear z-near
			 * \param _fZFar z-far
			 * \brief create scale matrix according to values passed in parameter
			 */
			void CreateProjection(T _fFov, T _fAspect, T _fZNear, T _fZFar);

			/**
			 * \fn void CreateProjection(T _fLeft, T _fRight, T _fBottom, T _fTop, T _fZNear, T _fZFar);
			 * \param _fLeft left
			 * \param _fRight right
			 * \param _fBottom bottom
			 * \param _fTop top
			 * \param _fZNear near
			 * \param _fZFar far
			 * \brief create orthogonal projection matrix according to values passed in parameter
			 */
			void CreateProjection(T _fLeft, T _fRight, T _fBottom, T _fTop, T _fZNear, T _fZFar);

			/**
			 * \fn void SetIdentity();
			 * \brief Set identity matrix. 1 on diagonal , 0 otherwise
			 */
			void SetIdentity();

			/**
			 * \fn operator T* ();
			 * \return the matrix value
			 * \brief retrieve the matrix value in a array of T.
			 * Useful for casting
			 */
			operator T* ();

			/**
			 * \fn operator const T* () const
			 * \return the matrix value
			 * \brief retrieve the matrix value in a array of T.
			 * Useful for casting
			 */
			operator const T* () const;

			/**
			 * \fn const T & operator () (uint32 i, uint32 j) const;
			 * \return the value corresponding to the indexes
			 */
			const T & operator () (uint32 i, uint32 j) const;

			/**
			 * \fn T & operator () (uint32 i, uint32 j);
			 * \return the value corresponding to the indexes
			 */
			T & operator () (uint32 i, uint32 j);

			/**
			 * \fn void Translate(T _x, T _y, T _z);
			 * \param _x x value translated
			 * \param _y y value translated
			 * \param _z z value translated
			 * \brief add translation according to values in parameter
			 */
			void Translate(T _x, T _y, T _z);

			/**
			 * \fn void Translate(const Vector3D<T> & _v);
			 * \param _v translate current position according to vector _v
			 * \brief add translation according to vector passed in parameter
			 */
			void Translate(const Vector3D<T> & _v);

			/**
			 * \fn void SetPostion(T _x, T _y, T _z);
			 * \param _x new x value
			 * \param _y new y value
			 * \param _z new z value
			 * \brief set new position according to values in parameter
			 */
			void SetPostion(T _x, T _y, T _z);

			/**
			 * \fn void SetPostion(const Point3D<T> & _p);
			 * \param _p new position
			 * \brief set new position according to 3d point passed in parameter
			 */
			void SetPostion(const Point3D<T> & _p);

			/**
			 * \fn Point3D<T> GetPosition() const;
			 * \return position
			 */
			Point3D<T> GetPosition() const;

			/**
			 * \fn void SetRotation( const Quaternion & _q);
			 * \param _q new rotation
			 * \brief set new rotation according to quaternion passed in parameter
			 */
			void SetRotation(const Quaternion & _q);

			/**
			 * \fn void SetRotation( const Vector3D<T> & _fAxis, float _fAngle);
			 * \param _fAxis new axis rotation
			 * \param _fAngle new angle rotation
			 * \brief set new rotation according to axis and angle passed in parameter
			 */
			void SetRotation(const Vector3D<T> & _fAxis, float _fAngle);

			/**
			 * \fn Quaternion GetRotation();
			 * \return rotation
			 */
			Quaternion GetRotation();

			/**
			 * \fn void GetRotation( Vector3D<T> & _fAxis, float & _fAngle );
			 * \param _fAxis axis
			 * \param _fAngle angle
			 */
			void GetRotation(Vector3D<T> & _fAxis, float & _fAngle);

			/**
			 * \fn void SetScale( T _x, T _y, T _z );
			 * \param _x new scale factor for x axis
			 * \param _y new scale factor for y axis
			 * \param _z new scale factor for z axis
			 * \brief set new scale according parameters
			 */
			void SetScale(T _x, T _y, T _z);

			/**
			 * \fn void SetScale( const Vector3D<T> & _scale );
			 * \param _scale new scale factor
			 * \brief set new scale Vector 3d passed in parameter
			 */
			void SetScale(const Vector3D<T> & _scale);

			/**
			 * \fn void GetScale( T & _x, T & _y, T & _z ) const;
			 * \param _x fill with scale factor for x axis
			 * \param _y fill with scale factor for y axis
			 * \param _z fill with scale factor for z axis
			 * \brief retrieve scale factor
			 */
			void GetScale(T & _x, T & _y, T & _z) const;

			/**
			 * \fn Vector3D<T> GetScale() const;
			 * \return scale factor corresponding to matrix
			 * \brief retrieve scale factor
			 */
			Vector3D<T> GetScale() const;

			/**
			 * \fn T Determinant() const;
			 * \return determinant of the matrix
			 */
			T Determinant() const;

			/**
			 * \fn Matrix<T> GetInverse() const;
			 * \return the inverse of the matrix
			 */
			Matrix<T> GetInverse() const;

			/**
			 * \fn Matrix<T> GetTranspose() const;
			 * \return compute the transposed matrix
			 */
			Matrix<T> GetTranspose() const;

			/**
			 * \fn template < class S > friend Matrix<S> operator * (const Matrix<S> & _m1, const Matrix<S> & _m2);
			 * \param _m1 matrix
			 * \param _m2 matrix
			 * \return the multiplication between m1 and m2
			 */
			template < class S > friend Matrix<S> operator * (const Matrix<S> & _m1, const Matrix<S> & _m2);

			/**
			 * \fn template < class S > friend Matrix<S> operator *(const Matrix<S> & _m, const S & _v);
			 * \param _m matrix
			 * \param _v T value
			 * \return the multiplication between all value of _m and _v
			 */
			template < class S > friend Matrix<S> operator *(const Matrix<S> & _m, const S & _v);

			/**
			 * \fn template < class S > friend Matrix<S> operator /(const Matrix<S> & _m, const S & _v);
			 * \param _m matrix
			 * \param _v value
			 * \return the division between all value of _m and _v
			 */
			template < class S > friend Matrix<S> operator /(const Matrix<S> & _m, const S & _v);

			//template < class T > friend Point4D<T> operator *(const Matrix4x4<T> & m, const Point4D<T> & p);
			//template < class T > friend Point3D<T> operator *(const Matrix4x4<T> & m, const Point3D<T> & p);

			/**
			 * \fn template < class S > friend bool operator == (const Matrix<S> & _m1, const Matrix<S> & _m2);
			 * \param _m1 matrix
			 * \param _m2 matrix
			 * \return true if _m1 is equal to _m2
			 */
			template < class S > friend bool operator == (const Matrix<S> & _m1, const Matrix<S> & _m2);

			/**
			 * \fn template < class S > friend bool operator != (const Matrix<S> & _m1, const Matrix<S> & _m2);
			 * \param _m1 matrix
			 * \param _m2 matrix
			 * \return true if _m1 is not equal to _m2
			 */
			template < class S > friend bool operator != (const Matrix<S> & _m1, const Matrix<S> & _m2);

		private:

			/**
			 * \fn void Set( T _tValues[16] );
			 * \param _tValues values to copy to matrix values
			 */
			void Set(T _tValues[16]);

			/**
			 * \fn void Set( T _tValues[4][4] );
			 * \param _tValues values to copy to matrix values
			 */
			void Set(T _tValues[4][4]);

			/**
			 * \fn T Minor(const uint32 r0, const uint32 r1, const uint32 r2, const uint32 c0, const uint32 c1, const uint32 c2) const;
			 * \param _r0 index of the 1st row of the sub matrix
			 * \param _r1 index of the 2nd row of the sub matrix
			 * \param _r2 index of the 3rd row of the sub matrix
			 * \param _c0 index of the 1st col of the sub matrix
			 * \param _c1 index of the 2nd col of the sub matrix
			 * \param _c2 index of the 3rd col of the sub matrix
			 * \return the determinant of the sub matrix
			 */
			T Minor(const uint32 r0, const uint32 r1, const uint32 r2, const uint32 c0, const uint32 c1, const uint32 c2) const;

			/** Matrix values */
			T m_tValues[16];
		};

		/** Matrix 4x4 of float */
		typedef Matrix<float>	Matrix4f;


		template < class T >
		Matrix< T >::Matrix()
		{
			SetIdentity();
		}

		template < class T >
		Matrix< T >::Matrix(const Matrix<T> & _mMatrix)
		{
			memcpy(m_tValues, _mMatrix.m_tValues, 16 * sizeof(T));
		}

		template < class T >
		Matrix< T >::Matrix(T _tab[16])
		{
			Set(_tab);
		}

		template < class T >
		Matrix< T >::Matrix(T _tab[4][4])
		{
			Set(_tab);
		}

		template < class T >
		T Matrix<T>::Minor(const uint32 r0, const uint32 r1, const uint32 r2, const uint32 c0, const uint32 c1, const uint32 c2) const
		{
			return	(*this)(r0, c0) * ((*this)(r1, c1) * (*this)(r2, c2) - (*this)(r2, c1) * (*this)(r1, c2)) -
				(*this)(r0, c1) * ((*this)(r1, c0) * (*this)(r2, c2) - (*this)(r2, c0) * (*this)(r1, c2)) +
				(*this)(r0, c2) * ((*this)(r1, c0) * (*this)(r2, c1) - (*this)(r2, c0) * (*this)(r1, c1));
		}

		template < class T >
		Matrix< T >::~Matrix()
		{

		}

		template < class T >
		void Matrix< T >::Set(T _tValues[16])
		{
			memcpy(m_tValues, _tValues, 16 * sizeof(T));
		}

		template < class T >
		void Matrix< T >::Set(T _tValues[4][4])
		{
			memcpy(m_tValues, _tValues, 16 * sizeof(T));
		}

		template < class T >
		void Matrix<T>::CreateTranslation(T _x, T _y, T _z)
		{
			SetIdentity();
			Translate(_x, _y, _z);
		}

		template < class T >
		void Matrix<T>::CreateTranslation(const Vector3D<T> & _translation)
		{
			SetIdentity();
			Translate(_translation);
		}

		template < class T >
		void Matrix<T>::CreateRotation(const Quaternion & _q)
		{
			SetIdentity();
			SetRotation(_q);
		}


		template < class T >
		void Matrix<T>::CreateRotation(const Vector3D<float> & _fAxis, float _fAngle)
		{
			SetIdentity();
			SetRotation(_fAxis, _fAngle);
		}

		template < class T >
		void Matrix<T>::CreateScale(const Vector3D<T> & _scale)
		{
			SetIdentity();
			SetScale(_scale);
		}


		template < class T >
		void Matrix<T>::CreateScale(T _scaleX, T _scaleY, T _scaleZ)
		{
			SetIdentity();
			SetScale(_scaleX, _scaleY, _scaleZ);
		}


		template < class T >
		void Matrix<T>::CreateProjection(T _fFov, T _fAspect, T _fZNear, T _fZFar)
		{
			memset(m_tValues, 0, sizeof(T)* 16);

			T yScale = 1 / tan(_fFov / 2);
			T xScale = yScale / _fAspect;

			(*this)(0, 0) = xScale;
			(*this)(1, 1) = yScale;
			(*this)(2, 2) = _fZFar / (_fZFar - _fZNear);
			(*this)(2, 3) = 1;
			(*this)(3, 2) = -_fZNear * _fZFar / (_fZFar - _fZNear);
		}

		template < class T >
		void Matrix<T>::CreateProjection(T _fLeft, T _fRight, T _fBottom, T _fTop, T _fZNear, T _fZFar)
		{
			memset(m_tValues, 0, sizeof(T)* 16);

			(*this)(0, 0) = (T)2 / (_fRight - _fLeft);
			(*this)(1, 1) = (T)2 / (_fTop - _fBottom);
			(*this)(2, 2) = (T)1 / (_fZFar - _fZNear);
			(*this)(3, 0) = (_fRight + _fLeft) / (_fLeft - _fRight);
			(*this)(3, 1) = (_fTop + _fBottom) / (_fBottom - _fTop);
			(*this)(3, 2) = _fZNear / (_fZNear - _fZFar);
			(*this)(3, 3) = 1;
		}

		template < class T >
		void Matrix< T >::SetIdentity()
		{
			for (uint32 i = 0; i < 16; ++i)
				m_tValues[i] = (T)(i % 5 == 0 ? 1 : 0);
		}

		template < class T >
		Matrix< T >::operator T* ()
		{
			return m_tValues;
		}

		template < class T >
		Matrix< T >::operator const T* () const
		{
			return m_tValues;
		}

		template < class T >
		T & Matrix< T >::operator () (uint32 i, uint32 j)
		{
			//LOG_ASSERT( (i < 4), "index out of bounds");
			//LOG_ASSERT( (j < 4), "index out of bounds");
			return m_tValues[4 * i + j];
		}

		template < class T >
		const T & Matrix< T >::operator () (uint32 i, uint32 j) const
		{
			//LOG_ASSERT( (i < 4), "index out of bounds");
			//LOG_ASSERT( (j < 4), "index out of bounds");
			return m_tValues[4 * i + j];
		}

		template < class T >
		void Matrix< T >::Translate(T _x, T _y, T _z)
		{
			m_tValues[12] += _x;
			m_tValues[13] += _y;
			m_tValues[14] += _z;
		}

		template < class T >
		void Matrix< T >::Translate(const Vector3D<T> & _v)
		{
			m_tValues[12] += _v.x;
			m_tValues[13] += _v.y;
			m_tValues[14] += _v.z;
		}

		template < class T >
		void Matrix< T >::SetPostion(T _x, T _y, T _z)
		{
			m_tValues[12] = _x;
			m_tValues[13] = _y;
			m_tValues[14] = _z;
		}

		template < class T >
		void Matrix< T >::SetPostion(const Point3D<T> & _p)
		{
			m_tValues[12] = _p.x;
			m_tValues[13] = _p.y;
			m_tValues[14] = _p.z;
		}

		template < class T >
		Point3D< T > Matrix< T >::GetPosition() const
		{
			return Point3D< T >(m_tValues[12], m_tValues[13], m_tValues[14]);
		}

		template < class T >
		void Matrix<T>::SetRotation(const Quaternion & _q)
		{
			float sX, sY, sZ;
			GetScale(sX, sY, sZ);
			_q.GetMatrix((*this));
			SetScale(sX, sY, sZ);
		}

		template < class T >
		void Matrix<T>::SetRotation(const Vector3D<T> & _fAxis, float _fAngle)
		{
			SetRotation(Quaternion(_fAxis, _fAngle));
		}

		template < class T >
		Quaternion Matrix<T>::GetRotation()
		{
			Quaternion q;
			float sX, sY, sZ;
			GetScale(sX, sY, sZ);
			SetScale(1, 1, 1);
			q.CreateFromMatrix((*this));
			SetScale(sX, sY, sZ);
			return q;
		}

		template < class T >
		void Matrix<T>::GetRotation(Vector3D<T> & _fAxis, float & _fAngle)
		{
			Quaternion q = GetRotation();
			q.GetAxis(_fAxis, _fAngle);
		}


		template < class T >
		void Matrix<T>::SetScale(T _x, T _y, T _z)
		{
			Vector3D<T> scaleX = Vector3D<T>(m_tValues[0], m_tValues[1], m_tValues[2]);
			scaleX = scaleX.Normalize() * _x;
			Vector3D<T> scaleY = Vector3D<T>(m_tValues[4], m_tValues[5], m_tValues[6]);
			scaleY = scaleY.Normalize() * _y;
			Vector3D<T> scaleZ = Vector3D<T>(m_tValues[8], m_tValues[9], m_tValues[10]);
			scaleZ = scaleZ.Normalize() * _z;
			m_tValues[0] = scaleX.x;
			m_tValues[1] = scaleX.y;
			m_tValues[2] = scaleX.z;
			m_tValues[4] = scaleY.x;
			m_tValues[5] = scaleY.y;
			m_tValues[6] = scaleY.z;
			m_tValues[8] = scaleZ.x;
			m_tValues[9] = scaleZ.y;
			m_tValues[10] = scaleZ.z;
		}

		template < class T >
		void Matrix<T>::SetScale(const Vector3D<T> & _scale)
		{
			SetScale(_scale.x, _scale.y, _scale.z);
		}


		template < class T >
		void Matrix<T>::GetScale(T & _x, T & _y, T & _z) const
		{
			Vector3D<T> scaleX(m_tValues[0], m_tValues[1], m_tValues[2]);
			_x = scaleX.GetLength();
			Vector3D<T> scaleY(m_tValues[4], m_tValues[5], m_tValues[6]);
			_y = scaleY.GetLength();
			Vector3D<T> scaleZ(m_tValues[8], m_tValues[9], m_tValues[10]);
			_z = scaleZ.GetLength();
		}

		template < class T >
		Vector3D<T> Matrix<T>::GetScale() const
		{
			T x, y, z;
			GetScale(x, y, z);
			return Vector3D<T>(x, y, z);
		}

		template < class T >
		T Matrix<T>::Determinant() const
		{
			return	(*this)(0, 0) * Minor(1, 2, 3, 1, 2, 3) -
				(*this)(0, 1) * Minor(1, 2, 3, 0, 2, 3) +
				(*this)(0, 2) * Minor(1, 2, 3, 0, 1, 3) -
				(*this)(0, 3) * Minor(1, 2, 3, 0, 1, 2);
		}

		template < class T >
		Matrix<T> Matrix<T>::GetTranspose() const
		{
			Matrix<T> mTrans;
			for (uint32 i = 0; i < 4; ++i)
			for (uint32 j = 0; j < 4; ++j)
				mTrans(i, j) = (*this)(j, i);
			return mTrans;

		}

		template < class T >
		Matrix<T>Matrix<T>::GetInverse() const
		{
			Matrix<T> mInv;
			uint32 r[] = { 1, 2, 3 };
			for (uint32 i = 0; i < 4; ++i)
			{
				uint32 c[] = { 1, 2, 3 };
				for (uint32 j = 0; j < 4; ++j)
				{
					T det = Minor(r[0], r[1], r[2], c[0], c[1], c[2]);
					mInv(j, i) = pow(-1.0f, (int)(i + j)) * det;
					if (j < 3)
						c[j] = j;
				}
				if (i < 3)
					r[i] = i;
			}

			return mInv / Determinant();
		}
	}
}

/**
 * \fn template < class S > Math::M3D::Matrix<S> operator * (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2);
 * \param _m1 matrix
 * \param _m2 matrix
 * \return the multiplication between m1 and m2
 */
template < class S >
Math::M3D::Matrix<S> operator * (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2)
{
	Math::M3D::Matrix<S> m;
	for(uint32 i = 0; i < 4; ++i)
	{
		for(uint32 j = 0; j < 4; ++j)
		{
			m(i, j) = 0;
			for(uint32 k = 0; k < 4; ++k)
				m(i, j) += _m1(i,k) * _m2(k,j);
		}
	}
	return m;
}

/**
 * \fn template < class S > Math::M3D::Matrix<S> operator *(const Math::M3D::Matrix<S> & _m, const S & _v);
 * \param _m matrix
 * \param _v T value
 * \return the multiplication between all value of _m and _v
 */
template < class S > 
Math::M3D::Matrix<S> operator * (const Math::M3D::Matrix<S> & _m, const S & _v)
{
	Math::M3D::Matrix<S> mRes;
	for(uint32 i = 0; i < 4; ++i)
		for(uint32 j = 0; j < 4; ++j)
			mRes(i, j) = _m(i, j) * _v;

	return mRes;
}


/**
 * \fn template < class S > Math::M3D::Matrix<S> operator /(const Math::M3D::Matrix<S> & _m, const S & _v);
 * \param _m matrix
 * \param _v value
 * \return the division between all value of _m and _v
 */
template < class S > 
Math::M3D::Matrix<S> operator / (const Math::M3D::Matrix<S> & _m, const S & _v)
{
	Math::M3D::Matrix<S> mRes;
	for(uint32 i = 0; i < 4; ++i)
		for(uint32 j = 0; j < 4; ++j)
			mRes(i, j) = _m(i, j) / _v;

	return mRes;
}

/**
 * \fn template < class S > bool operator == (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2);
 * \param _m1 matrix
 * \param _m2 matrix
 * \return true if _m1 is equal to _m2
 */
template < class S >
bool operator == (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2)
{
	for(uint32 i = 0; i < 16; ++i)
		if( _m1.m_tValues[i] != _m2.m_tValues[i] )
			return false;

	return true;
}

/**
 * \fn template < class S > bool operator != (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2);
 * \param _m1 matrix
 * \param _m2 matrix
 * \return true if _m1 is not equal to _m2
 */
template < class S >
bool operator != (const Math::M3D::Matrix<S> & _m1, const Math::M3D::Matrix<S> & _m2)
{
	return !(_m1 == _m2);
}

#endif // _MATRIX_
